/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.propagation.numerical;

import ore.Attitude;
import ore.AttitudeLaw;
import ore.ForceModel;
import ore.Frame;
import ore.SpacecraftState;
import ore.attitudes.InertialLaw;
import ore.errors.OrekitException;
import ore.errors.PropagationException;
import ore.orbits.EquinoctialOrbit;
import ore.propagation.BoundedPropagator;
import ore.propagation.Propagator;
import ore.propagation.events.AdaptedEventDetector;
import ore.propagation.events.EventDetector;
import ore.propagation.sampling.AdaptedStepHandler;
import ore.propagation.sampling.OrekitFixedStepHandler;
import ore.propagation.sampling.OrekitStepHandler;
import ore.propagation.sampling.OrekitStepNormalizer;
import ore.time.AbsoluteDate;

import org.apache.commons.math.ode.DerivativeException;
import org.apache.commons.math.ode.FirstOrderDifferentialEquations;
import org.apache.commons.math.ode.FirstOrderIntegrator;
import org.apache.commons.math.ode.IntegratorException;
import org.apache.commons.math.ode.events.EventHandler;
import org.apache.commons.math.ode.sampling.DummyStepHandler;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.List;

/** 
 * This class propagates {@link ore.Orbit orbits} using
 * numerical integration.
 * 
 * Numerical propagation is much more accurate than analytical
 * propagation like for example {@link
 * ore.propagation.analytical.KeplerianPropagator keplerian} or {@link
 * ore.propagation.analytical.EcksteinHechlerPropagator
 * Eckstein-Hechler}, but requires a few more steps to set up to be
 * used properly.  Whereas analytical propagators are configured only
 * thanks to their various constructors and can be used immediately
 * after construction, numerical propagators configuration involve
 * setting several parameters between construction time and
 * propagation time.
 * 
 * The configuration parameters that can be set are the following.
 * 
 * <ul>
 *   <li>the initial spacecraft state ({@link #setInitialState(SpacecraftState)})</li>
 *   <li>the central attraction coefficient ({@link #setMu(double)})</li>
 *   <li>the various force models ({@link #addForceModel(ForceModel)},
 *   {@link #removeForceModels()})</li>
 *   <li>the discrete events that should be triggered during propagation
 *   ({@link #addEventDetector(EventDetector)},
 *   {@link #clearEventsDetectors()})</li>
 *   <li>the binding logic with the rest of the application ({@link #setSlaveMode()},
 *   {@link #setMasterMode(double, OrekitFixedStepHandler)}, {@link
 *   #setMasterMode(OrekitStepHandler)}, {@link #setEphemerisMode()}, {@link
 *   #getGeneratedEphemeris()})</li>
 * </ul>
 * 
 * From these configuration parameters, only the initial state is mandatory. If the
 * central attraction coefficient is not explicitly specified, the one used to define
 * the initial orbit will be used. However, specifying only the initial state and
 * perhaps the central attraction coefficient would mean the propagator would use only
 * keplerian forces. In this case, the simpler {@link
 * ore.propagation.analytical.KeplerianPropagator KeplerianPropagator} class would
 * perhaps be more effective. The propagator is only in one mode at a time.</p>
 * <p>The underlying numerical integrator set up in the constructor can also be configured.
 * Typical tuning parameters for adaptive stepsize integrators are the min, max and perhaps
 * start step size as well as the absolute and/or relative errors thresholds. The state
 * that is seen by the integrator is a simple seven elements double array. The six first
 * elements are the {@link EquinoctialOrbit equinoxial orbit parameters} (a, e<sub>x</sub>,
 * e<sub>y</sub>, h<sub>x</sub>, h<sub>y</sub>, l<sub>v</sub>) in meters and radians, and
 * the last element is the mass in kilograms. The following code snippet shows a typical
 * setting for Low Earth Orbit propagation.
 * 
 * <pre>
 * final double minStep  = 0.001;
 * final double maxStep  = 1000;
 * final double initStep = 60;
 * final double[] absTolerance = {
 *     0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001
 * };
 * final double[] relTolerance = {
 *     1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7
 * };
 * AdaptiveStepsizeIntegrator integrator =
 *     new DormandPrince853Integrator(minStep, maxStep, absTolerance, relTolerance);
 * integrator.setInitialStepSize(initStep);
 * propagator = new NumericalPropagator(integrator);
 * </pre>
 * 
 * The same propagator can be reused for several orbit extrapolations,
 * by resetting the initial state without modifying the other
 * configuration parameters.
 * 
 * This class is not multi- thread safe.
 * 
 * @see SpacecraftState
 * @see ForceModel
 * @see OrekitStepHandler
 * @see OrekitFixedStepHandler
 * @see IntegratedEphemeris
 * @see TimeDerivativesEquations
 *
 * @author Mathieu Rom&eacute;ro
 * @author Luc Maisonobe
 * @author Guylaine Prat
 * @author Fabien Maussion
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class NumericalPropagator 
    extends Object
    implements Propagator
{

    /** 
     * Attitude law.
     */
    private AttitudeLaw attitudeLaw;
    /** 
     * Central body gravitational constant.
     */
    private double mu;
    /** 
     * Force models used during the extrapolation of the Orbit.
     */
    private final List<ForceModel> forceModels;
    /** 
     * Event detectors not related to force models.
     */
    private final List<EventDetector> detectors;
    /** 
     * State vector.
     */
    private final double[] state;
    /** 
     * Start date.
     */
    private AbsoluteDate startDate;
    /** 
     * Initial state to propagate.
     */
    private SpacecraftState initialState;
    /** 
     * Current state to propagate.
     */
    private SpacecraftState currentState;
    /** 
     * Integrator selected by the user for the orbital extrapolation process.
     */
    private transient FirstOrderIntegrator integrator;
    /** 
     * Counter for differential equations calls.
     */
    private int calls;
    /** 
     * Gauss equations handler.
     */
    private TimeDerivativesEquations adder;
    /** 
     * Propagator mode handler.
     */
    private ModeHandler modeHandler;
    /** 
     * Current mode.
     */
    private int mode;


    /** 
     * Create a new instance of NumericalPropagator, based on orbit
     * definition mu.  After creation, the instance is empty, i.e. the
     * attitude law is set to an unspecified default law and there are
     * no perturbing forces at all.  This means that if {@link
     * #addForceModel addForceModel} is not called after creation, the
     * integrated orbit will follow a keplerian evolution only.
     * 
     * @param integrator Numerical integrator to use for propagation.
     */
    public NumericalPropagator(FirstOrderIntegrator integrator) {
        super();
        this.mu           = Double.NaN;
        this.forceModels  = new ArrayList<ForceModel>();
        this.detectors    = new ArrayList<EventDetector>();
        this.startDate    = new AbsoluteDate();
        this.attitudeLaw  = InertialLaw.EME2000_ALIGNED;
        this.state        = new double[7];
        this.setIntegrator(integrator);
        this.setSlaveMode();
    }


    /** 
     * @param integrator Numerical integrator to use for propagation.
     */
    public void setIntegrator(final FirstOrderIntegrator integrator) {
        this.integrator = integrator;
    }
    /** 
     * @param mu Central attraction coefficient, &mu; (m^3/s^2)
     */
    public void setMu(double mu) {
        this.mu = mu;
    }
    /**
     * @return &mu; central attraction coefficient (m^3/s^2)
     */
    public double getMu() {
        return this.mu;
    }
    /** 
     * @param attitudeLaw Attitude law
     */
    public void setAttitudeLaw(AttitudeLaw attitudeLaw) {
        this.attitudeLaw = attitudeLaw;
    }
    /**
     * {@inheritDoc}
     */
    public void addEventDetector(final EventDetector detector) {
        this.detectors.add(detector);
    }
    /**
     * {@inheritDoc}
     */
    public Collection<EventDetector>getEventsDetectors() {
        return Collections.unmodifiableCollection(this.detectors);
    }
    /**
     * {@inheritDoc}
     */
    public void clearEventsDetectors() {
        this.detectors.clear();
    }
    /** 
     * Add a force model to the global perturbation model.
     * 
     * For a force model associated to discrete events, these events
     * will be handled automatically, they must <strong>not</strong>
     * be added using the {@link #addEventDetector(EventDetector)
     * addEventDetector} method.
     * 
     * If this method is not called, the integrated orbit will follow
     * a keplerian evolution only.
     * 
     * @param model Perturbing {@link ForceModel} to add
     */
    public void addForceModel(ForceModel model) {
        this.forceModels.add(model);
    }
    /** 
     * Remove all perturbing force models from the global perturbation
     * model.
     * 
     * Once all perturbing forces have been removed (and as long as no
     * new force model is added), the integrated orbit will follow a
     * keplerian evolution only.
     */
    public void removeForceModels() {
        this.forceModels.clear();
    }
    /**
     * {@inheritDoc}
     */
    public int getMode() {
        return this.mode;
    }
    /**
     * {@inheritDoc}
     */
    public void setSlaveMode() {
        this.integrator.addStepHandler(DummyStepHandler.getInstance());
        this.modeHandler = null;
        this.mode = SLAVE_MODE;
    }
    /**
     * {@inheritDoc}
     */
    public void setMasterMode(double h, OrekitFixedStepHandler handler) {
        this.setMasterMode(new OrekitStepNormalizer(h, handler));
    }
    /**
     * {@inheritDoc}
     */
    public void setMasterMode(OrekitStepHandler handler) {
        AdaptedStepHandler wrapped = new AdaptedStepHandler(handler);
        this.integrator.addStepHandler(wrapped);
        this.modeHandler = wrapped;
        this.mode = MASTER_MODE;
    }
    /**
     * {@inheritDoc}
     */
    public void setEphemerisMode() {
        final IntegratedEphemeris ephemeris = new IntegratedEphemeris();
        this.integrator.addStepHandler(ephemeris);
        this.modeHandler = ephemeris;
        this.mode = EPHEMERIS_GENERATION_MODE;
    }
    /**
     * {@inheritDoc}
     */
    public BoundedPropagator getGeneratedEphemeris()
        throws IllegalStateException
    {
        if (this.mode != EPHEMERIS_GENERATION_MODE)
            throw OrekitException.createIllegalStateException("propagator is not in batch mode");
        else
            return (IntegratedEphemeris) this.modeHandler;
    }
    /** 
     * @param initialState Initial state
     */
    public void setInitialState(SpacecraftState initialState) {
        if (Double.isNaN(this.mu))
            this.mu = initialState.getMu();
        else
            this.initialState = initialState;
    }
    /**
     * {@inheritDoc} 
     */
    public SpacecraftState propagate(AbsoluteDate finalDate)
        throws PropagationException
    {
        if (this.initialState == null)
            throw new PropagationException("initial state not specified for orbit propagation");

        else if (this.initialState.getDate().equals(finalDate)) {
            /*
             * Don't extrapolate
             */
            return this.initialState;
        }
        else if (this.integrator == null)
            throw new PropagationException("ODE integrator not set for orbit propagation");
        else {
            try {
                /*
                 * Space dynamics view
                 */
                this.startDate  = initialState.getDate();
                if (this.modeHandler != null) {
                    this.modeHandler.initialize(startDate, initialState.getFrame(), mu, attitudeLaw);
                }

                final EquinoctialOrbit initialOrbit =
                    new EquinoctialOrbit(initialState.getOrbit());

                this.currentState =
                    new SpacecraftState(initialOrbit, initialState.getAttitude(), initialState.getMass());

                this.adder = new TimeDerivativesEquations(initialOrbit);

                if (initialState.getMass() <= 0.0)
                    throw new IllegalArgumentException("Mass is null or negative");
                else {
                    /*
                     * Mathematical view
                     */
                    final double t0 = 0;
                    final double t1 = finalDate.durationFrom(startDate);
                    /*
                     * Map state to array
                     */
                    state[0] = initialOrbit.getA();
                    state[1] = initialOrbit.getEquinoctialEx();
                    state[2] = initialOrbit.getEquinoctialEy();
                    state[3] = initialOrbit.getHx();
                    state[4] = initialOrbit.getHy();
                    state[5] = initialOrbit.getLv();
                    state[6] = initialState.getMass();

                    this.integrator.clearEventHandlers();
                    /*
                     * Set up events related to force models
                     */
                    for (ForceModel forceModel : forceModels) {
                        EventDetector[] modelDetectors = forceModel.getEventsDetectors();
                        if (modelDetectors != null) {
                            for (EventDetector detector : modelDetectors) {
                                setUpEventDetector(detector);
                            }
                        }
                    }
                    /*
                     * Set up events added by user
                     */
                    for (EventDetector detector : detectors) {
                        setUpEventDetector(detector);
                    }
                    /*
                     * Mathematical integration
                     */
                    final double stopTime = integrator.integrate(new DifferentialEquations(), t0, state, t1, state);
                    /*
                     * Back to space dynamics view
                     */
                    AbsoluteDate date = new AbsoluteDate(startDate, stopTime);
                    double a    = state[0];
                    double ex   = state[1];
                    double ey   = state[2];
                    double hx   = state[3];
                    double hy   = state[4];
                    double lv   = state[5];
                    double mass = state[6];

                    EquinoctialOrbit orbit = new EquinoctialOrbit(a, ex, ey, hx, hy, lv, 
                                                                  EquinoctialOrbit.TRUE_LATITUDE_ARGUMENT,
                                                                  initialOrbit.getFrame(), date, this.mu);

                    return new SpacecraftState(orbit,
                                               attitudeLaw.getState(date,
                                                                    orbit.getPositionVelocity(),
                                                                    orbit.getFrame()),
                                               mass);
                }
            }
            catch (OrekitException oe) {
                throw new PropagationException(oe.getMessage(), oe);
            }
            catch (DerivativeException de) {
                /*
                 * Recover a possible embedded PropagationException
                 */
                for (Throwable t = de; t != null; t = t.getCause()) {
                    if (t instanceof PropagationException) {
                        throw (PropagationException) t;
                    }
                }
                throw new PropagationException(de.getMessage(), de);
            }
            catch (IntegratorException ie) {
                /*
                 * Recover a possible embedded PropagationException
                 */
                for (Throwable t = ie; t != null; t = t.getCause()) {
                    if (t instanceof PropagationException) {
                        throw (PropagationException) t;
                    }
                }
                throw new PropagationException(ie.getMessage(), ie);
            }
        }
    }
    /** 
     * The number of calls is reset each time the {@link
     * #propagate(AbsoluteDate)} method is called.
     * 
     * @return The number of calls to the differential equations
     * computation method.
     */
    public int getCalls() {
        return this.calls;
    }
    /** 
     * Wrap an Orekit event detector and register it to the
     * integrator.
     * 
     * @param osf event handler to wrap
     */
    private void setUpEventDetector(EventDetector osf) {
        EventHandler handler = new AdaptedEventDetector(osf, startDate, this.mu,
                                                        this.initialState.getFrame(), 
                                                        this.attitudeLaw);
        this.integrator.addEventHandler(handler,
                                        osf.getMaxCheckInterval(),
                                        osf.getThreshold(),
                                        osf.getMaxIterationCount());
    }
    /** 
     * Internal class for differential equations representation.
     */
    private class DifferentialEquations 
        extends Object
        implements FirstOrderDifferentialEquations
    {

        public DifferentialEquations() {
            super();
            NumericalPropagator.this.calls = 0;
        }


        /**
         * {@inheritDoc}
         */
        public int getDimension() {
            return 7;
        }
        /**
         * {@inheritDoc}
         */
        public void computeDerivatives(final double t, final double[] y, final double[] yDot)
            throws DerivativeException
        {
            try {
                /*
                 * Update space dynamics view
                 */
                NumericalPropagator.this.currentState = this.mapState(t, y, 
                                                                      NumericalPropagator.this.startDate, 
                                                                      NumericalPropagator.this.currentState.getFrame());
                /*
                 * Compute cartesian coordinates
                 */
                if (NumericalPropagator.this.currentState.getMass() <= 0.0) {
                    throw OrekitException.createIllegalArgumentException("spacecraft mass becomes negative (m: {0})",
                                                                         NumericalPropagator.this.currentState.getMass());
                }
                else {
                    /*
                     * Initialize derivatives
                     */
                    NumericalPropagator.this.adder.initDerivatives(yDot, (EquinoctialOrbit) currentState.getOrbit());
                    /*
                     * Compute the contributions of all perturbing forces
                     */
                    for (ForceModel forceModel : NumericalPropagator.this.forceModels) {
                        forceModel.addContribution(NumericalPropagator.this.currentState, NumericalPropagator.this.adder);
                    }
                    /*
                     * Finalize derivatives by adding the Kepler contribution
                     */
                    NumericalPropagator.this.adder.addKeplerContribution();

                    ++(NumericalPropagator.this.calls);
                }
            }
            catch (OrekitException oe) {
                throw new DerivativeException(oe.getMessage());
            }
        }
        /** 
         * Convert state array to space dynamics objects (AbsoluteDate
         * and OrbitalParameters).
         * 
         * @param t Integration time (s)
         * @param y State as a flat array
         * @param referenceDate Reference date from which t is counted
         * @param frame Frame in which integration is performed
         * 
         * @return State corresponding to the flat array as a space dynamics object
         * 
         * @exception OrekitException The attitude state cannot be
         * determined by the attitude law
         */
        private SpacecraftState mapState(final double t, final double [] y,
                                         AbsoluteDate referenceDate, Frame frame)
            throws OrekitException
        {
            double a    = y[0];
            double ex   = y[1];
            double ey   = y[2];
            double hx   = y[3];
            double hy   = y[4];
            double lv   = y[5];
            double mass = y[6];
            /*
             * Convert mathematical data to space dynamics objects
             */
            AbsoluteDate date = new AbsoluteDate(referenceDate, t);
            EquinoctialOrbit orbit = new EquinoctialOrbit(a, ex, ey, hx, hy, lv, 
                                                          EquinoctialOrbit.TRUE_LATITUDE_ARGUMENT,
                                                          frame, date, NumericalPropagator.this.mu);
            Attitude attitude =
                attitudeLaw.getState(date, orbit.getPositionVelocity(), frame);

            return new SpacecraftState(orbit, attitude, mass);
        }
    }
}
